# -*- coding: utf-8 -*-

import serial
import os
import sys
import math
import time
import rospy
from std_msgs.msg import Header

# 打开串口
ser = serial.Serial(sys.argv[1], 921600)
try:
    rospy.init_node('control_command', anonymous=True)
except rospy.exceptions.ROSException as e:
    if str(e) == "rospy.init_node() has already been called with different arguments: ('imu_node', ['serial_data.py', '/home/huarsuma/dataset/test'], True, None, False, False)":
        pass
    else:
        raise e


def ros_pub_command(command_list):
    hex_str = ''.join(format(num, 'X').zfill(2) for num in command_list)
    pub = rospy.Publisher('command', Header, queue_size=10)
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = hex_str
    #print(hex_str)
    pub.publish(header)


# 待发送的数据
if(sys.argv[2] == '-v' or sys.argv[2] == '-V' or sys.argv[2] == '--v' or sys.argv[2] == '--V'):
    speed = float(sys.argv[3]) * 2*3.1415926536  # 速度，单位是rpm
    speed_high = (int(speed * 100) >> 8) & 0xFF  # 取高八位
    speed_low = int(speed * 100) & 0xFF  # 取低八位
    data = [0xAA, 0xAF, 0x05, 0x13, 0x02, speed_high, speed_low]
    checksum = sum(data) & 0xFF
    data.append(checksum)
    data_bytes = bytes(data)
    
    ros_pub_command(data)
    
    ser.write(data_bytes)
    ser.close()
elif(sys.argv[2] == '-p' or sys.argv[2] == '-P' or sys.argv[2] == '--p' or sys.argv[2] == '--P'):
    position = float(sys.argv[3])      # 位置，单位是rad
    position_high = int(position * 100) >> 8  # 取高八位
    position_low = int(position * 100) & 0xFF  # 取低八位
    data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
    checksum = sum(data) & 0xFF
    data.append(checksum)
    data_bytes = bytes(data)
    
    ros_pub_command(data)
    
    ser.write(data_bytes)
    ser.close()
elif(sys.argv[2] == '-s' or sys.argv[2] == '-S' or sys.argv[2] == '--s' or sys.argv[2] == '--S'):
    # 设置正弦运动参数
    amplitude = float(sys.argv[3]) / 180 * math.pi  # 最大转动角度为30度
    period = float(sys.argv[4])  # 周期为2秒
    num_steps = 100  # 每个周期内的步数
    delay = period / num_steps  # 步长的延迟时间
    # 循环发送位置控制指令
    while True:
        for i in range(num_steps):
            t = i * delay  # 当前时间
            position = math.pi + amplitude * math.sin(2 * math.pi * t / period)
            position_high = int(position * 100) >> 8  # 取高八位
            position_low = int(position * 100) & 0xFF  # 取低八位
            
            # 构造指令数据
            data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
            checksum = sum(data) & 0xFF
            data.append(checksum)
            data_bytes = bytes(data)
            ser.write(data_bytes)
            
            ros_pub_command(data)
            
            # 延迟
            time.sleep(delay)
elif(sys.argv[2] == '-r' or sys.argv[2] == '-R' or sys.argv[2] == '--r' or sys.argv[2] == '--R'):
    amplitude = float(sys.argv[3]) / 180 * math.pi  # 最大转动角度为30度
    period = float(sys.argv[4])  # 周期为2秒
    stop = float(sys.argv[5])
    num_steps = 100  # 每个周期内的步数
    delay = period / num_steps  # 步长的延迟时间
    # 循环发送位置控制指令
    while True:
        for i in range(num_steps):
            t = i * delay  # 当前时间
            position = math.pi + amplitude * t / period
            position_high = int(position * 100) >> 8  # 取高八位
            position_low = int(position * 100) & 0xFF  # 取低八位
            
            # 构造指令数据
            data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
            checksum = sum(data) & 0xFF
            data.append(checksum)
            data_bytes = bytes(data)
            ser.write(data_bytes)
            
            ros_pub_command(data)
            
            # 延迟
            time.sleep(delay)
        time.sleep(stop)
        for i in range(num_steps):
            t = i * delay  # 当前时间
            position = math.pi + amplitude * (period - t) / period
            position_high = int(position * 100) >> 8  # 取高八位
            position_low = int(position * 100) & 0xFF  # 取低八位
            
            # 构造指令数据
            data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
            checksum = sum(data) & 0xFF
            data.append(checksum)
            data_bytes = bytes(data)
            ser.write(data_bytes)
            
            ros_pub_command(data)
            
            # 延迟
            time.sleep(delay)
        time.sleep(stop)
        for i in range(num_steps):
            t = i * delay  # 当前时间
            position = math.pi - amplitude * t / period
            position_high = int(position * 100) >> 8  # 取高八位
            position_low = int(position * 100) & 0xFF  # 取低八位
            
            # 构造指令数据
            data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
            checksum = sum(data) & 0xFF
            data.append(checksum)
            data_bytes = bytes(data)
            ser.write(data_bytes)
            
            ros_pub_command(data)
            
            # 延迟
            time.sleep(delay)
        time.sleep(stop)
        for i in range(num_steps):
            t = i * delay  # 当前时间
            position = math.pi - amplitude * (period - t) / period
            position_high = int(position * 100) >> 8  # 取高八位
            position_low = int(position * 100) & 0xFF  # 取低八位
            
            # 构造指令数据
            data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
            checksum = sum(data) & 0xFF
            data.append(checksum)
            data_bytes = bytes(data)
            ser.write(data_bytes)
            
            ros_pub_command(data)
            
            # 延迟
            time.sleep(delay)
        time.sleep(stop)
elif(sys.argv[2] == '-rv' or sys.argv[2] == '-rV' or sys.argv[2] == '--rv' or sys.argv[2] == '--rV'):
    speed = float(sys.argv[3]) * 2*3.1415926536  # 速度，单位是rpm
    period = 2  # 周期为2秒
    num_steps = 100  # 每个周期内的步数
    delay = period / num_steps  # 步长的延迟时间
    for i in range(num_steps):
        speed_temp = i * speed / num_steps
        speed_high = (int(speed_temp * 100) >> 8) & 0xFF  # 取高八位
        speed_low = int(speed_temp * 100) & 0xFF  # 取低八位
        data = [0xAA, 0xAF, 0x05, 0x13, 0x02, speed_high, speed_low]
        checksum = sum(data) & 0xFF
        data.append(checksum)
        data_bytes = bytes(data)
        ser.write(data_bytes)
        
        ros_pub_command(data)
        
        # 延迟
        time.sleep(delay)
elif(sys.argv[2] == '-rp' or sys.argv[2] == '-rP' or sys.argv[2] == '--rp' or sys.argv[2] == '--rP'):
    present_position = 0.0
    with open('/home/huarsuma/jetson_datasave/last_position.txt', 'r') as file:
        present_position = float(file.read())
    position = float(sys.argv[3])      # 位置，单位是rad
    period = float(sys.argv[4])  # 周期为2秒
    num_steps = 100  # 每个周期内的步数
    delay = period / num_steps  # 步长的延迟时间
    for i in range(num_steps):
        position_temp = present_position + i * (position - present_position) / num_steps
        position_high = (int(position_temp * 100) >> 8) & 0xFF  # 取高八位
        position_low = int(position_temp * 100) & 0xFF  # 取低八位
        data = [0xAA, 0xAF, 0x05, 0x11, 0x02, position_high, position_low]
        checksum = sum(data) & 0xFF
        data.append(checksum)
        data_bytes = bytes(data)
        ser.write(data_bytes)
        
        ros_pub_command(data)
        
        # 延迟
        time.sleep(delay)
    with open('/home/huarsuma/jetson_datasave/last_position.txt', 'w') as file:
        file.write(str(position))

